#include "RS485.h"

extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;

uint8_t g_rxBuf[20];
uint8_t g_txBuf[20];
uint8_t rxCnt = 0;

serial serial_t = { 0 };

// 计算校验和
uint8_t GetCheckSum(uint8_t *buffer,uint8_t size)
{
	uint16_t sum = 0;
	for(int i = 0; i < size; i++) {
		sum += buffer[i];  //计算累加值
	}
	return(sum & 0xFF);     //返回校验和
}

// 读取电机参数
void ReadParameters(uint8_t slaveAddr, uint8_t cmd)
{
	g_txBuf[0] = DOWN_FRAME_HEADER; // 帧头
	g_txBuf[1] = slaveAddr; // 从机地址
	g_txBuf[2] = cmd; // 功能码
	g_txBuf[3] = GetCheckSum(g_txBuf, 3); // 计算校验和
	HAL_UART_Transmit(&huart2, g_txBuf, 4, 0xff); // 串口发出读取实时位置指令
}

// 设置系统参数指令功能码后需要写入数据无法封装成单一函数，需要用到再设置即可
// 设置工作电流
void SetMode(uint8_t slaveAddr, uint16_t milliamperes)
{
	g_txBuf[0] = DOWN_FRAME_HEADER; // 帧头
	g_txBuf[1] = slaveAddr; // 从机地址
	g_txBuf[2] = SET_WORK_CURRENT; // 功能码
	g_txBuf[3] = milliamperes >> 8; // 电流高八位
	g_txBuf[4] = milliamperes; // 电流高八位
	g_txBuf[5] = GetCheckSum(g_txBuf, 5); // 计算校验和
	HAL_UART_Transmit(&huart2, g_txBuf, 6, 0xff); // 串口发出读取实时位置指令
}

// 控制电机运行无法封装成单一函数，需要用到再设置即可
// 电机以模式3运行
void positionMode3Run(uint8_t slaveAddr, uint16_t speed, uint8_t acc, int32_t absAxis)
{
  g_txBuf[0] = DOWN_FRAME_HEADER; // 帧头
  g_txBuf[1] = slaveAddr; // 从机地址
  g_txBuf[2] = RELATIVE_COORAINATE; // 功能码
  g_txBuf[3] = (speed >> 8) & 0x00FF; // 速度高8位
  g_txBuf[4] = speed & 0x00FF; // 速度低8位
  g_txBuf[5] = acc; // 加速度
  g_txBuf[6] = (absAxis >> 24)&0xFF; // 相对坐标 bit31 - bit24
  g_txBuf[7] = (absAxis >> 16)&0xFF; // 相对坐标 bit23 - bit16
  g_txBuf[8] = (absAxis >> 8)&0xFF; // 相对坐标 bit15 - bit8
  g_txBuf[9] = (absAxis >> 0)&0xFF; // 相对坐标 bit7 - bit0
  g_txBuf[10] = GetCheckSum(g_txBuf,10); // 计算校验和

  for(int i=0; i<11;i++) {
	  printf("g_txBuf[%d] = %d\r\n", i, g_txBuf[i]);
  }
  HAL_UART_Transmit(&huart2, g_txBuf, 11, 0xff);
}
// 电机以模式4运行
void positionMode4Run(uint8_t slaveAddr, uint16_t speed, uint8_t acc, int32_t absAxis)
{
  g_txBuf[0] = DOWN_FRAME_HEADER; // 帧头
  g_txBuf[1] = slaveAddr; // 从机地址
  g_txBuf[2] = ABSOLUTE_COORAINATE; // 功能码
  g_txBuf[3] = (speed >> 8) & 0x00FF; // 速度高8位
  g_txBuf[4] = speed & 0x00FF; // 速度低8位
  g_txBuf[5] = acc; // 加速度
  g_txBuf[6] = (absAxis >> 24)&0xFF; // 绝对坐标 bit31 - bit24
  g_txBuf[7] = (absAxis >> 16)&0xFF; // 绝对坐标 bit23 - bit16
  g_txBuf[8] = (absAxis >> 8)&0xFF; // 绝对坐标 bit15 - bit8
  g_txBuf[9] = (absAxis >> 0)&0xFF; // 绝对坐标 bit7 - bit0
  g_txBuf[10] = GetCheckSum(g_txBuf,10); // 计算校验和

  HAL_UART_Transmit(&huart2, g_txBuf, 11, 0xff);
}

// 等待从机响应并接收指定长读返回数据包
uint8_t WaitForACK(uint8_t len)
{
	uint8_t retVal = 0; // 返回值
//	unsigned long sTime; // 计时起始时刻
//	unsigned long time; // 当前时刻
	uint8_t rxByte;

//	sTime = millis(); // 获取当前时刻
	rxCnt = 0; // 接收计数值置0
	while(1) {
		// 串口接收到数据
		if (serial_t.Uart_RX_Flag == 1) {
			rxByte = serial_t.rxByte; // 读取1字节数据
			if(rxCnt != 0) {
				g_rxBuf[rxCnt++] = rxByte; // 存储数据
			} else if(rxByte == 0xFB) { // 判断是否帧头
				rxCnt = 0;
				g_rxBuf[rxCnt++] = rxByte; // 存储帧头
			}
		}
		// 接收完成
		if(rxCnt == len) {
			if(g_rxBuf[len - 1] == GetCheckSum(g_rxBuf, len - 1)) {
				retVal = HAL_OK; // 校验正确
				break; // 退出循环
			} else {
				rxCnt = 0; // 校验错误，重新接收应答
			}
		}

//		time = millis();
//		if((time - sTime) > 3000) // 判断是否超时
//		{
//			retVal = HAL_ERROR;
//			break; // 超时退出
//		}
	}
	return(retVal);
}
